#ifndef __AFCQUEUE_H__
#define __AFCQUEUE_H__

////////////////////////////////////////////////////////////////////////////////
// File    : AFCQueue.h
// Author  : Mr Michael Gorelik;  email: smgorelik@gmail.com;
// Written : 21 May 2012
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the New BSD License.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of 
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License 
// along with this program; if not, write to the Free Software Foundation
// Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
////////////////////////////////////////////////////////////////////////////////
// TODO:	
////////////////////////////////////////////////////////////////////////////////

#include "../framework/cpp_framework.h"
#include "ITest.h"

extern volatile int	_gIsStopThreads;
extern int _gAddRemoveMode;

#define USE_TIMESTAMPS_ARRAY
#define PARALLEL_EXTRACT_MIN

#define QUEUES_PER_CONS 4
#define JUMP_DIST 8

#define HELPERS_QUEUE_SIZE 4194304 
#define HELPERS_QUEUE_SIZE_1 4194303 

#define USE_64

#if defined(USE_64)
typedef _u64 TimeType;
# define MAX_TIME_VALUE ULONG_MAX
#else
typedef _u32 TimeType;
# define MAX_TIME_VALUE UINT_MAX
#endif

using namespace CCP;
//Helper Structures

#define _INIT_SIZE 262144

//NodeArrayBlock representing local array block of items
struct NodeArrayBlock{
	TimeType*		_timestamp_arr;
	int*			_values_arr;
	volatile struct NodeArrayBlock * _next;
	int				_in;
	int				_out;
	bool			_free;

	NodeArrayBlock(){
		_timestamp_arr		= (TimeType*)malloc(_INIT_SIZE*sizeof(TimeType));
		_values_arr			= (int*)malloc(_INIT_SIZE*sizeof(int));
		if (!_timestamp_arr || !_values_arr)
		{
			printf("no memory to allocate for node");
		}
		_in					= 0;
		_out				= 0;
		_free				= false;
		_next				= null;
	}

	virtual ~NodeArrayBlock(){
		if (_timestamp_arr){
			free((void*)_timestamp_arr);
			_timestamp_arr=null;
		}
		if (_values_arr){
			free((void*)_values_arr);
			_values_arr=null;
		}
		_next = null;
	}
};

//Snap Shot of local array
struct ArraySnapShot{
	volatile NodeArrayBlock*	_head;
	volatile NodeArrayBlock*	_tail;
	int							_out;
	ArraySnapShot(){
		_out		= 0;
		_head		= null;
		_tail		= null;
	}
};

extern int _gNumAddThreads;
extern int _gNumRemoveThreads;

class AFCQueue : public ITest {
private:


	//snapshot array of local arrays
	ArraySnapShot volatile ** collectedItems;
	//snapshot array of local arrays
	ArraySnapShot volatile ** staticCollectedItems;
	//timestamps array
	TimeType volatile timestamps[_MAX_THREADS]; 

	//request types
	static final int _HELP_EXTRACT_COMPLEX		= (INT_MIN+4);

	//queues count
	volatile int numOfQueues;
	//Producer structure
	struct EnqueueSlotInfo {
		EnqueueSlotInfo* volatile	_next;			//when null not connected
		volatile NodeArrayBlock*	_head;			//pointer to the first local array block
		volatile NodeArrayBlock*	_tail;			//pointer to the last local array block
		volatile int				_out;			//the last index that was read in prev safe collect
		EnqueueSlotInfo(bool is_allocate = true) {
			_next       = null;
			if (is_allocate){
				_head		= new NodeArrayBlock();	
				_head->_next =  new NodeArrayBlock();
				_head->_next->_next =  new NodeArrayBlock();
				_head->_next->_next->_next =  new NodeArrayBlock();
			}else
			{
				_head		= null;
			}
			_tail			= _head;
			_out			= 0;
		}
		~EnqueueSlotInfo(){
			while (_head){
				volatile NodeArrayBlock* tmp = _head;
				_head = _head->_next;
				tmp->_next = null;
				delete tmp;
			}
			_head=null;
			_tail=null;
		}
	};
	//Consumer structure
	struct DequeueSlotInfo {
		DequeueSlotInfo* volatile	_next;			//when null not connected
		volatile int 		_req_ans;				//here 1 can post the request and wait for answer
		volatile int 		_saved_ans;				//here 1 can post a value in case other work need to be done
		volatile int 		_startIndex;
		volatile int 		_endIndex;
		volatile int 		_helperIndex;


		DequeueSlotInfo() {
			_req_ans		= _NULL_VALUE;
			_saved_ans		= _NULL_VALUE;
			_next			= null;
			_startIndex		= 0;
			_endIndex		= 0;
			_helperIndex	= 0;
		}
	};
	//producers list
	CCP::ThreadLocal<EnqueueSlotInfo*>	_tls_enqueue_slot_info;
	CCP::AtomicReference<EnqueueSlotInfo>	_tail_enqueue_slot;
	//consumers list
	CCP::ThreadLocal<DequeueSlotInfo*>	_tls_dequeue_slot_info;
	CCP::AtomicReference<DequeueSlotInfo>	_tail_dequeue_slot;

	//list helper function --------------------------
	//Register as a producer
	EnqueueSlotInfo* get_new_enqueue_slot() {
		EnqueueSlotInfo* my_slot= new EnqueueSlotInfo();
		_tls_enqueue_slot_info.set(my_slot);

		EnqueueSlotInfo* curr_tail;
		do {
			curr_tail = _tail_enqueue_slot.get();
			my_slot->_next = curr_tail;
		} while(false == _tail_enqueue_slot.compareAndSet(curr_tail, my_slot));
		return my_slot;
	}
	//Register as a consumer
	DequeueSlotInfo* get_new_dequeue_slot() {
		DequeueSlotInfo* my_slot= new DequeueSlotInfo();
		_tls_dequeue_slot_info.set(my_slot);

		DequeueSlotInfo* curr_tail;
		do {
			curr_tail = _tail_dequeue_slot.get();
			my_slot->_next = curr_tail;
		} while(false == _tail_dequeue_slot.compareAndSet(curr_tail, my_slot));
		return my_slot;
	}
	//count non-empty queues, shift empty to the end
	inline_ int countNonEmptyQueues(volatile ArraySnapShot* static_arr[],volatile ArraySnapShot* arr[],int size){
		int k,j,i = size-1;
		volatile ArraySnapShot* key;
		//empty queues last
		for (j=0;j<size;j++){
			arr[j] = static_arr[j];
		}
		for (;i>=0;i--){
			if (arr[i]->_out != arr[i]->_tail->_out || arr[i]->_tail != arr[i]->_head )
				break;
		}
		for (k = 0; k < i; ++k){
			if (arr[k]->_out == arr[k]->_tail->_out && arr[k]->_tail == arr[k]->_head)
			{
				key = arr[k];
				arr[k] = arr[i];
				arr[i] = key;
				while (arr[i]->_out == arr[i]->_tail->_out && arr[i]->_tail == arr[i]->_head)
					i--;
			}
		}
		return i+1;
	}
	//fields --------------------------------------
	AtomicInteger	CACHE_ALIGN _fc_lock;
	final int		_NUM_REP;
	final int		_REP_THRESHOLD;

	/************Main Algorithm********************/
	inline_ void SafeCollect(){

		int i=0, in;
		TimeType maxtimestamp=0, tmpTime;
		EnqueueSlotInfo* curr_slot,*head_slot = _tail_enqueue_slot.get();
		volatile NodeArrayBlock* tail = null, *head=null;
		volatile ArraySnapShot * local_arr = null;
		curr_slot = head_slot;
		numOfQueues=0;

		for(i=0 ; curr_slot->_next ; i++){
			//If the tail was changed during snapshot
			tail = curr_slot->_tail;
			in = tail->_in;
			head = curr_slot->_head;

			local_arr = staticCollectedItems[i];
			if (local_arr->_tail){
				local_arr->_head = local_arr->_tail;
			}else{
				local_arr->_head = head;
			}

			local_arr->_tail = tail;
			local_arr->_out = curr_slot->_out;
			curr_slot->_out = in;
			tail->_out = in;  

			if (local_arr->_out != in || local_arr->_head != tail){ //items where added : queue is not empty
				tmpTime = tail->_timestamp_arr[in-1];
				if (maxtimestamp < tmpTime)
					maxtimestamp = tmpTime;
			}
			curr_slot = curr_slot->_next;
		}

		if (!maxtimestamp){
			return;
		}

		if (!single_producer)
		{
			collectChanges(head_slot,staticCollectedItems,maxtimestamp);
			numOfQueues = countNonEmptyQueues(staticCollectedItems,collectedItems,i);
		}else{
			collectedItems[0] = staticCollectedItems[0];
			numOfQueues=1;
		}

	}

	//Second Pass over snapshot of SafeCollect
	inline_ void collectChanges(EnqueueSlotInfo* curr_slot,
		ArraySnapShot volatile** prev_items, 
		TimeType maxtimestamp)
	{

		volatile NodeArrayBlock* tail_prev;
		volatile NodeArrayBlock* tail_new;
		volatile ArraySnapShot * local_arr;
		TimeType* time_arr;
		int prev_out,in,max_in;

		for(int i=0; curr_slot->_next ; i++){
			tail_new = curr_slot->_tail;
			in = tail_new->_in;

			local_arr = prev_items[i];
			tail_prev = local_arr->_tail;
			prev_out = local_arr->_tail->_out;

			//if queue is empty or the first item that was added is bigger the maxtime 
			if (prev_out == in && tail_prev==tail_new) //no item was added
			{
				curr_slot = curr_slot->_next;
				continue;
			}

			if (prev_out < _INIT_SIZE){
				if (tail_prev->_timestamp_arr[prev_out] > maxtimestamp){ // first added item's timestmap bigger then max
					curr_slot = curr_slot->_next;
					continue;
				}
			}else{
				if (tail_prev->_next->_timestamp_arr[0] > maxtimestamp){
					curr_slot = curr_slot->_next;
					continue;
				}
			}
			//if the last item that was added is less than maxtime, add all items
			if (tail_new->_timestamp_arr[in-1] <= maxtimestamp){
				tail_new->_out = in;
				curr_slot->_out = in;
				local_arr->_tail = tail_new;
				curr_slot = curr_slot->_next;
				continue;
			}

			//collect all items smaller then maxtime
			time_arr = tail_prev->_timestamp_arr;
			tail_prev==tail_new ? max_in = in : max_in=_INIT_SIZE;

			while((prev_out+JUMP_DIST) < max_in)
			{
				if (time_arr[prev_out+JUMP_DIST] > maxtimestamp)
					break;
				prev_out+=JUMP_DIST+1;
			}

			do{
				if(prev_out < _INIT_SIZE){
					if (time_arr[prev_out] > maxtimestamp)
						break;
					prev_out++;
				}else{
					if (tail_prev->_next->_timestamp_arr[0] > maxtimestamp)
						break;
					tail_prev = tail_prev->_next;
					time_arr = tail_prev->_timestamp_arr;
					prev_out = 1;
				}
			}while(prev_out!=in || tail_prev!=tail_new);
			//update safe collect
			tail_prev->_out = prev_out;
			curr_slot->_out = prev_out;
			local_arr->_tail = tail_prev;
			curr_slot = curr_slot->_next;
		}
		return ;
	}
	//collected consumers
	volatile DequeueSlotInfo* consumers[_MAX_THREADS];
	volatile int numOfConsumers;
	volatile int servedConsumers;

	NodeArrayBlock volatile * heads[_MAX_THREADS];
	NodeArrayBlock volatile *tails[_MAX_THREADS];
	int volatile outs[_MAX_THREADS];
	int volatile ins[_MAX_THREADS];
	TimeType volatile timestamps_arr[_MAX_THREADS];
	int volatile values_arr[_MAX_THREADS];
	//serve Requests
	inline_ int serveRequests(int volatile * reqs[_MAX_THREADS],int num);
	int serveRequestsComplex(int volatile * reqs[_MAX_THREADS],int num);

	//Extract items from SafeCollect to a local queue (helper_queue_time,helper_queue_value), 
	void ExtractValuesComplex(int startIndex, int endIndex,
		TimeType volatile *helper_queue_time,int volatile* helper_queue_value,
		volatile int& in_local_index,volatile int& out_local_index);

	//Helper flags
	volatile int real_helpers_count;

	volatile bool requests_complex;

	volatile int in_first;
	volatile int out_first;
	volatile int in_second;
	volatile int out_second;
	volatile bool first_helper_finished;
	volatile bool second_helper_finished;
	TimeType volatile *f_helper_queue_time;
	TimeType volatile *s_helper_queue_time;
	int volatile* f_helper_queue_value;
	int volatile* s_helper_queue_value;
	volatile TimeType tmp_queue_time_first;
	volatile TimeType tmp_queue_time_sec;
	int numOfRegisteredConsumers;
	int numOfRegisteredProducers;
	volatile bool single_consumer;
	volatile bool single_producer;
	int volatile * requests[_MAX_THREADS];


	NodeArrayBlock volatile * single_head;
	NodeArrayBlock volatile * single_tail;
	int volatile single_out;
	int volatile* single_in;
	int* single_value_arr;
	int in_consumer;
	int out_consumer;


	int dupArrayIndex[_MAX_THREADS];
	int dupIndex;
	int dupSize;

	inline_ int flat_combining_single_consumer(){
		int val=0,index=0;

		if (dupIndex < dupSize && dupArrayIndex[dupIndex] < numOfQueues){
			index = dupArrayIndex[dupIndex++];
		}else{
			if (numOfQueues==0){
				SafeCollect();

				if (numOfQueues==0)
					return 0;

				for (int i=0 ; i < numOfQueues; i++){
					heads[i] = collectedItems[i]->_head;
					outs[i]  = collectedItems[i]->_out;
					tails[i] = collectedItems[i]->_tail;
					ins[i]   = collectedItems[i]->_tail->_out;
					if (outs[i]==_INIT_SIZE){
						heads[i]->_free=true;
						heads[i] = heads[i]->_next;
						outs[i]=0;
						timestamps_arr[i] = heads[i]->_timestamp_arr[0];
					}else{
						timestamps_arr[i] = heads[i]->_timestamp_arr[outs[i]];
					}
				}	
			}
			dupIndex=0;
			TimeType minTime = timestamps_arr[0];
			index = 0;

			for (int i=0; i< numOfQueues; i++){
				if (timestamps_arr[i] <= minTime)
				{
					if (timestamps_arr[i] == minTime){
						dupArrayIndex[dupSize++] = i;
						continue;
					}
					minTime = timestamps_arr[i];
					index = i;
					dupSize=0;
				}
			}
		}
		val = heads[index]->_values_arr[outs[index]++];

		if (outs[index] != _INIT_SIZE){}
		else{
			heads[index]->_free = true;
			heads[index] = heads[index]->_next;
			if (heads[index]==null)
			{
				tails[index] = null;
				ins[index]=0;
			}
			outs[index] = 0;
		}

		if (ins[index] != outs[index] ||
			heads[index]!=tails[index])
		{
			timestamps_arr[index] = heads[index]->_timestamp_arr[outs[index]];
		}else{
			if (--numOfQueues != index){
				heads[index]					= heads[numOfQueues];
				tails[index]					= tails[numOfQueues];
				outs[index]						= outs[numOfQueues];
				ins[index]						= ins[numOfQueues];
				timestamps_arr[index]			= timestamps_arr[numOfQueues];
			}
		}
		return val;
	}

	inline_ void flat_combining_single_producer(){

		Memory::read_barrier();	
		//Collect the consumers for one round
		DequeueSlotInfo* curr_deq_slot = _tail_dequeue_slot.get();

		if (single_head==null){
			EnqueueSlotInfo* curr_enq_slot = _tail_enqueue_slot.get();
			single_head = curr_enq_slot->_head;
			single_tail = curr_enq_slot->_tail;
			single_in = &curr_enq_slot->_head->_in;
			single_value_arr = curr_enq_slot->_head->_values_arr;
			single_out = 0;
		}

		if (null != curr_deq_slot->_next){
			do {
				volatile int& req = curr_deq_slot->_req_ans; 
				if(_DEQ_VALUE == req) {
					if (*single_in==single_out && single_head==single_tail)
					{
						req = 0;
						curr_deq_slot = curr_deq_slot->_next;
						continue;
					}

					if (single_out != _INIT_SIZE){}
					else{
						EnqueueSlotInfo* curr_enq_slot = _tail_enqueue_slot.get();
						single_out=0;
						NodeArrayBlock volatile* tmp = single_head;
						single_head = single_head->_next;
						curr_enq_slot->_head = single_head;
						tmp->_next = 0;
						delete tmp;
					}

					req = single_value_arr[single_out++];
				}
				curr_deq_slot = curr_deq_slot->_next;
			}while(null != curr_deq_slot->_next);
		}
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
			Memory::write_barrier();
#		endif
	}
	EnqueueSlotInfo* curr_single_enq_slot; 
	inline_ int flat_combining_single_producer_single_consumer(){

		Memory::read_barrier();	

		if (single_head==null){
			curr_single_enq_slot = _tail_enqueue_slot.get();
			single_head = curr_single_enq_slot->_head;
			single_in = &curr_single_enq_slot->_head->_in;
			single_value_arr = curr_single_enq_slot->_head->_values_arr;
			single_out = 0;
		}

		if (*single_in==single_out && single_head==curr_single_enq_slot->_tail)
			return 0;

		if (single_out != _INIT_SIZE){}
		else{
			single_head->_free = true;
			single_head = single_head->_next;
			single_out = 0;
		}

		return single_value_arr[single_out++];
	}

	void flat_combining(DequeueSlotInfo* local_slot) {
		int num_of_changes;
		DequeueSlotInfo* curr_slot=null;

		for (register int iTry=0;iTry<_NUM_REP; ++iTry) {

			Memory::read_barrier();	
			num_of_changes = 0;
			numOfConsumers = 0;
			//Collect the consumers for one round
			curr_slot = _tail_dequeue_slot.get();

#if defined(PARALLEL_EXTRACT_MIN)
			if (requests_complex==false && numOfQueues >= QUEUES_PER_CONS && numOfRegisteredConsumers >= QUEUES_PER_CONS){
				bool local_slot_visited = false;
				int middleQueue = numOfQueues>>1;

				while( real_helpers_count < 2 && null != curr_slot->_next){
					if (curr_slot==local_slot)
					{
						local_slot_visited = true;
						curr_slot = curr_slot->_next;
						continue;
					} 
					volatile int& req = curr_slot->_req_ans; 

					if(_DEQ_VALUE == req ) {
						consumers[real_helpers_count++] = curr_slot;
						requests[numOfConsumers++] = &curr_slot->_saved_ans;
					}
					curr_slot = curr_slot->_next;
				}
				if (real_helpers_count == 1){
					requests[numOfConsumers-1] = &(consumers[0]->_req_ans);
					real_helpers_count = 0;
				}else{
					tmp_queue_time_first= tmp_queue_time_sec = 0;
					in_first = out_first = 0;
					first_helper_finished=false;
					in_second = out_second = 0;
					second_helper_finished=false;
				}
				if (local_slot_visited)
					requests[numOfConsumers++] = &local_slot->_req_ans;
			}		
#endif			
			if (null != curr_slot->_next){
				do {
					volatile int& req = curr_slot->_req_ans; 
					if(_DEQ_VALUE == req) {
						requests[numOfConsumers++] = &req;
					}
					curr_slot = curr_slot->_next;
				}while(null != curr_slot->_next);
			}

			if (!numOfConsumers)
				break;
			//If helper consumers are merging the queues, serve the requests using they local merged queues

			servedConsumers = serveRequests(requests,numOfConsumers);

			num_of_changes+=servedConsumers;
			//There are no more items in SafeCollect or in the merged queues (in case of a complex request)
			while (servedConsumers!=numOfConsumers){
				//No elements in prev SafeCollect, try to do SafeCollect

				SafeCollect();
				//SafeCollect is empty, serve consumers with 0=empty
				if (!numOfQueues){
					while (servedConsumers<numOfConsumers)
						*(requests[servedConsumers++])=0;
				}else{
					requests_complex=false;real_helpers_count=0;

					for (int i=0 ; i < numOfQueues; i++){
						heads[i] = collectedItems[i]->_head;
						outs[i]  = collectedItems[i]->_out;
						tails[i] = collectedItems[i]->_tail;
						ins[i]   = collectedItems[i]->_tail->_out;
						if (outs[i]==_INIT_SIZE){
							heads[i]->_free=true;
							heads[i] = heads[i]->_next;
							outs[i]=0;
							timestamps_arr[i] = heads[i]->_timestamp_arr[0];
						}else{
							timestamps_arr[i] = heads[i]->_timestamp_arr[outs[i]];
						}
					}
					//SafeCollect is not empty , serve the rest of the consumers
					servedConsumers+= serveRequests(&requests[servedConsumers],
						numOfConsumers - servedConsumers);
					num_of_changes=servedConsumers;
				}
			}
			if (requests_complex==false && real_helpers_count==2)
			{
				volatile DequeueSlotInfo* tmpSlot = consumers[0];
				int middle = numOfQueues>>1;
				tmpSlot->_helperIndex = 0;
				tmpSlot->_startIndex = 0;
				tmpSlot->_endIndex = middle;

				tmpSlot = consumers[1];
				tmpSlot->_helperIndex = 1;
				tmpSlot->_startIndex = middle;
				tmpSlot->_endIndex = numOfQueues;

				Memory::write_barrier();
				tmpSlot->_req_ans = _HELP_EXTRACT_COMPLEX;
				consumers[0]->_req_ans = _HELP_EXTRACT_COMPLEX;
				requests_complex=true;
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
				Memory::write_barrier();
#		endif
				break;
			}
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
			Memory::write_barrier();
#		endif
			if(num_of_changes < _REP_THRESHOLD)
				break;
		}
	}

	inline_ void flat_combining_parallel() {
		int num_of_changes;
		do {

			Memory::read_barrier();	
			num_of_changes = 0;
			numOfConsumers = 0;
			//Collect the consumers for one round
			DequeueSlotInfo* curr_slot = _tail_dequeue_slot.get();

			while(null != curr_slot->_next) {
				volatile int& req = curr_slot->_req_ans; 
				if(_DEQ_VALUE == req) {
					requests[numOfConsumers++] = &req;
				}
				curr_slot = curr_slot->_next;
			}

			if (!numOfConsumers)
				break;
			//If helper consumers are merging the queues, serve the requests using they local merged queues

			servedConsumers = serveRequestsComplex(requests,numOfConsumers);

			num_of_changes+=servedConsumers;
			//There are no more items in SafeCollect or in the merged queues (in case of a complex request)
			if (servedConsumers==numOfConsumers){} 
			else{
				do{
					//No elements in prev SafeCollect, try to do SafeCollect
					SafeCollect();
					//SafeCollect is empty, serve consumers with 0=empty
					if (!numOfQueues){
						while (servedConsumers<numOfConsumers)
							*(requests[servedConsumers++])=0;
					}else{
						requests_complex=false;real_helpers_count=0;

						for (int i=0 ; i < numOfQueues; i++){
							heads[i] = collectedItems[i]->_head;
							outs[i]  = collectedItems[i]->_out;
							tails[i] = collectedItems[i]->_tail;
							ins[i]   = collectedItems[i]->_tail->_out;
							if (outs[i]==_INIT_SIZE){
								heads[i]->_free=true;
								heads[i] = heads[i]->_next;
								outs[i]=0;
								timestamps_arr[i] = heads[i]->_timestamp_arr[0];
							}else{
								timestamps_arr[i] = heads[i]->_timestamp_arr[outs[i]];
							}
						}
						//SafeCollect is not empty , serve the rest of the consumers
						servedConsumers+= serveRequests(&requests[servedConsumers],
							numOfConsumers - servedConsumers);
						num_of_changes=servedConsumers;
					}
				}while(servedConsumers<numOfConsumers);
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
				Memory::write_barrier();
#		endif
				break;
			}
			if (_gIsStopThreads)
				break;
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
			Memory::write_barrier();
#		endif

		}while(true);
	}

public:
	//public operations ---------------------------
	AFCQueue() 
		:	_NUM_REP(_gNumRemoveThreads),
		_REP_THRESHOLD((int)(Math::ceil(_gNumRemoveThreads/(1.7))) + 1)
	{
		_tail_enqueue_slot.set(new EnqueueSlotInfo(false));
		_tail_dequeue_slot.set(new DequeueSlotInfo());
		numOfQueues = 0;

		collectedItems = new volatile ArraySnapShot*[_MAX_THREADS];
		staticCollectedItems = new volatile ArraySnapShot*[_MAX_THREADS];
		numOfRegisteredConsumers=_gNumRemoveThreads;
		numOfRegisteredProducers=_gNumAddThreads;
		requests_complex = false;
		real_helpers_count = 0;
		//Allocation of Safe collect array
		for (int i=0; i<_MAX_THREADS; i++){
			collectedItems[i] = new ArraySnapShot();
			staticCollectedItems[i] = new ArraySnapShot();
			timestamps[i]=0;
		}	
		//First Helper
		f_helper_queue_time = (TimeType*)malloc(HELPERS_QUEUE_SIZE*sizeof(TimeType));
		f_helper_queue_value = (int*)malloc(HELPERS_QUEUE_SIZE*sizeof(int));
		in_first=0;
		out_first=0;
		//Second Helper
		s_helper_queue_time = (TimeType*)malloc(HELPERS_QUEUE_SIZE*sizeof(TimeType));
		s_helper_queue_value = (int*)malloc(HELPERS_QUEUE_SIZE*sizeof(int));
		in_second=0;
		out_second=0;
		if (numOfRegisteredConsumers==1)
			single_consumer=true;
		else
			single_consumer=false;
		if (numOfRegisteredProducers==1)
			single_producer=true;
		else
			single_producer=false;

		single_head = null;
		single_tail = null;
		single_out = 0;
		single_in = null;
		single_value_arr = null;
		in_consumer = 0;
		out_consumer = 0;
		dupIndex=0;
		dupSize=0;
	}

	virtual ~AFCQueue() {
		free((void*)f_helper_queue_time);
		free((void*)f_helper_queue_value);
		free((void*)s_helper_queue_time);
		free((void*)s_helper_queue_value);
		for (int i=0; i<_MAX_THREADS; i++){
			delete staticCollectedItems[i];
		}	
		delete collectedItems;
		delete staticCollectedItems;
	}

	//enq ......................................................
	boolean add(final int iThread, final int inValue) {

		EnqueueSlotInfo* my_slot = _tls_enqueue_slot_info.get();
		if(null == my_slot){
			my_slot = get_new_enqueue_slot();
			return 0;
		}
		volatile NodeArrayBlock* tail = my_slot->_tail;
		volatile int& in = tail->_in;
		TimeType timestamp = timestamps[0];
		if (!single_producer){			
			for (int i=1; i< _gNumAddThreads;i++){
				if (timestamps[i] > timestamp)
					timestamp = timestamps[i];
			}

		}
		timestamps[iThread]=++timestamp;	
		if (in < _INIT_SIZE){
			tail->_timestamp_arr[in] = timestamp;
			tail->_values_arr[in] = inValue;
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
			Memory::write_barrier();
#		endif
			in++;
		}else{
			volatile NodeArrayBlock* tmp = null;
			if (tail->_next){
				tmp = tail->_next;
				tmp->_timestamp_arr[0] = timestamp;
				tmp->_values_arr[0] = inValue;
				tmp->_in = 1;
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
				Memory::write_barrier();
#		endif
				my_slot->_tail = tmp;
				return true;
			}		
			if (my_slot->_head->_free){
				tmp = my_slot->_head;
				my_slot->_head = my_slot->_head->_next;
				tmp->_next=0;
				tmp->_out=0;
				tmp->_free=false;
			}else{
				if (_gAddRemoveMode==2){

					//printf("+++START	Throttling on Thread id %d\n",iThread);
					do{
						CCP::Thread::sleep(10);
					}while(!my_slot->_head->_free);
					tmp = my_slot->_head;
					my_slot->_head = my_slot->_head->_next;
					tmp->_next=0;
					tmp->_out=0;
					tmp->_free=false;
					//printf("---END		Throttling on Thread id %d\n",iThread);	
				}
				if (tmp==null){
					tmp = new NodeArrayBlock();
					if (!tmp || !tmp->_timestamp_arr || !tmp->_values_arr)
					{
						printf("no memory for thread id %d\n",iThread);
						if (tmp)
							delete tmp;
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
						Memory::write_barrier();
#		endif
						return false;
					}
				}
			}

			tmp->_timestamp_arr[0] = timestamp;
			tmp->_values_arr[0] = inValue;
			tail->_next = tmp;
			tmp->_in = 1;
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
			Memory::write_barrier();
#		endif
			my_slot->_tail = tmp;
		}
		//Memory::write_barrier();
		return true;
	}

	//deq ......................................................
	int remove(final int iThread, final int inValue) {
		CasInfo& my_cas_info = _cas_info_ary[iThread];

		DequeueSlotInfo* my_slot = _tls_dequeue_slot_info.get();
		if(null == my_slot){
			my_slot = get_new_dequeue_slot();
			return 0;
		}

		int volatile& my_re_ans = my_slot->_req_ans;

		my_re_ans = _DEQ_VALUE;
		do {
			boolean is_cas = true;
			if(lock_fc(_fc_lock, is_cas)) {
				++(my_cas_info._succ);
				if (my_re_ans!= _DEQ_VALUE)
				{
					_fc_lock.set(0);
					goto CheckRequest;
				}
				machine_start_fc(iThread);
				if (single_consumer){
					if (single_producer)
						my_re_ans = flat_combining_single_producer_single_consumer();
					else
						my_re_ans = flat_combining_single_consumer();
				}else{

					if (!single_producer)
					{
						if (requests_complex)
							flat_combining_parallel();
						else
							flat_combining(my_slot);
					}else
						flat_combining_single_producer();
				}
				_fc_lock.set(0);
				machine_end_fc(iThread);
				++(my_cas_info._ops);
				return my_re_ans;
			} else {
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
				Memory::write_barrier();
#		endif
				if(!is_cas)
					++(my_cas_info._failed);
				while(_DEQ_VALUE == my_re_ans && 0 != _fc_lock.getNotSafe()) {
					thread_wait(iThread);
				}
				Memory::read_barrier();

CheckRequest:
				// Return a positive answer
				if(_NULL_VALUE <= my_re_ans) {
					++(my_cas_info._ops);
					return my_re_ans;
				}

#if defined(PARALLEL_EXTRACT_MIN)
				// Help in Extraction, be secondary helper
				if(_HELP_EXTRACT_COMPLEX == my_re_ans){	
					if (my_slot->_helperIndex==0){
						ExtractValuesComplex(my_slot->_startIndex,my_slot->_endIndex,
							f_helper_queue_time,f_helper_queue_value,in_first,out_first);
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
						Memory::write_barrier();
#		endif
						first_helper_finished = true;
					}
					else
					{
						ExtractValuesComplex(my_slot->_startIndex,my_slot->_endIndex,
							s_helper_queue_time,s_helper_queue_value,in_second,out_second);
#		if !defined(SPARC) && !defined(SPARC64)	
#		else
						Memory::write_barrier();
#		endif
						second_helper_finished = true;
					}
					++(my_cas_info._ops);				
					return my_slot->_saved_ans;
				}
#endif
			}
		} while(true);
	}

	void entry(final int iThread,bool flag = false) {
		if (flag)
			get_new_enqueue_slot();
		else
			get_new_dequeue_slot();
	}
	void exit(final int iThread,bool flag = false) {
		//get_new_slot();
	}
	//peek .....................................................
	int contain(final int iThread, final int inValue) {
		return _NULL_VALUE;
	}

	//general .....................................................
	int size() {
		return 0;
	}

	final char* name() {
		return "AFCQueue";
	}

};


inline_ int  AFCQueue::serveRequests(int volatile* reqs[_MAX_THREADS],int num)
{
	int size = 0,i,index;
	TimeType min_timestamp,tmp_min,tmp_time;
	bool is_singleQueue = false;

	if (numOfQueues){}
	else
		return size;

	min_timestamp = timestamps_arr[0];
	for(i=1,index=0; i < numOfQueues;i++){
		if (timestamps_arr[i] < min_timestamp){
			min_timestamp = timestamps_arr[i];
			index = i;
		}
	}
	tmp_min = min_timestamp;
	do{
SERVE_SIMPLE_DO:

		*(reqs[size++]) = heads[index]->_values_arr[outs[index]++];

		if (ins[index] != outs[index] ||
			heads[index]!=tails[index])
		{
			if (outs[index] != _INIT_SIZE){}
			else{
				heads[index]->_free = true;
				heads[index] = heads[index]->_next;
				outs[index] = 0;
			}
			timestamps_arr[index] = heads[index]->_timestamp_arr[outs[index]];
		}
		else{
			//if it was not the last local queue in the array of snapshots
			if (--numOfQueues != index){
				heads[index]					= heads[numOfQueues];
				tails[index]					= tails[numOfQueues];
				outs[index]						= outs[numOfQueues];
				ins[index]						= ins[numOfQueues];
				timestamps_arr[index]			= timestamps_arr[numOfQueues];
			}
		}

		if (size!=num && numOfQueues){}
		else
			return size;

		min_timestamp = timestamps_arr[0];
		for(i = 1,index=0; i < numOfQueues ;i++){
			tmp_time = timestamps_arr[i];
			if (tmp_time < min_timestamp){
				index = i;
				if (tmp_min==tmp_time)
					goto SERVE_SIMPLE_DO;
				min_timestamp = tmp_time;
			}
		}
		tmp_min = min_timestamp;
	}while(true);
}


int  AFCQueue::serveRequestsComplex(int volatile* reqs[_MAX_THREADS],int num)
{	
	int size = 0;
	TimeType min_timestamp;
	TimeType tmpTimestamp;

	if (numOfQueues){}
	else
		return size;

SERVE_COMPLEX_DO:
	min_timestamp = MAX_TIME_VALUE;

	if (tmp_queue_time_first){
		if (tmp_queue_time_first < min_timestamp){
			min_timestamp = tmp_queue_time_first;
		}
	}else{
		if (in_first != out_first)
		{
			tmpTimestamp = f_helper_queue_time[out_first];
			if (tmpTimestamp < min_timestamp){
				min_timestamp = tmpTimestamp;
			}
			tmp_queue_time_first= tmpTimestamp;
		}else{
			if (first_helper_finished) 
			{
				tmp_queue_time_first = MAX_TIME_VALUE;
			}
			else{
				while(in_first==out_first && !first_helper_finished)
				{
					if (_gIsStopThreads)
						return size;
				}
				if (in_first != out_first)
				{
					tmpTimestamp = f_helper_queue_time[out_first];
					if (tmpTimestamp < min_timestamp){
						min_timestamp = tmpTimestamp;
					}
					tmp_queue_time_first= tmpTimestamp;
				}else
					tmp_queue_time_first = MAX_TIME_VALUE;
			}				
		}
	}
	if (tmp_queue_time_sec){
		if (tmp_queue_time_sec < min_timestamp){
			goto SECOND_SMALLER_LABEL;
		}
	}else{
		if (in_second != out_second)
		{
			tmpTimestamp = s_helper_queue_time[out_second];
			if (tmpTimestamp < min_timestamp){
				goto SECOND_SMALLER_LABEL;
			}
			tmp_queue_time_sec= tmpTimestamp;
		}else{
			if (second_helper_finished) 
			{
				tmp_queue_time_sec = MAX_TIME_VALUE;
			}
			else{
				while(in_second==out_second && !second_helper_finished)
				{
					if (_gIsStopThreads)
						return size;
				}
				if (in_second != out_second)
				{
					tmpTimestamp = s_helper_queue_time[out_second];
					if (tmpTimestamp < min_timestamp){
						goto SECOND_SMALLER_LABEL;
					}
					tmp_queue_time_sec= tmpTimestamp;
				}else
					tmp_queue_time_sec = MAX_TIME_VALUE;
			}	
		}
	}

	if (min_timestamp == MAX_TIME_VALUE){
		numOfQueues = 0;
		return size;
	}

FIRST_SMALLER_LABEL:
	*(reqs[size++]) = f_helper_queue_value[out_first];
	if (out_first < HELPERS_QUEUE_SIZE_1)
		out_first++;
	else
		out_first = 0;
	if (in_first!=out_first)
	{
		tmp_queue_time_first = f_helper_queue_time[out_first];
		if (size==num)
			return size;
		if (tmp_queue_time_first <= tmp_queue_time_sec){
			goto FIRST_SMALLER_LABEL;
		}
		goto SECOND_SMALLER_LABEL;
	}
	else
		tmp_queue_time_first = 0;
	if (size==num)
		return size;

	goto SERVE_COMPLEX_DO;

SECOND_SMALLER_LABEL:
	*(reqs[size++]) = s_helper_queue_value[out_second];
	if (out_second < HELPERS_QUEUE_SIZE_1)
		out_second++;
	else
		out_second = 0;

	if (in_second!=out_second)
	{
		tmp_queue_time_sec = s_helper_queue_time[out_second];		
		if (size==num)
			return size;
		if (tmp_queue_time_sec <= tmp_queue_time_first){
			goto SECOND_SMALLER_LABEL;
		}
		goto FIRST_SMALLER_LABEL;
	}
	else
		tmp_queue_time_sec = 0;	
	if (size==num)
		return size;

	goto SERVE_COMPLEX_DO;
}

void AFCQueue::ExtractValuesComplex(int startIndex, int endIndex,
									TimeType volatile *helper_queue_time,
									int volatile* helper_queue_value,
									volatile int& in_local_index,
									volatile int& out_local_index)
{
	int size = 0,i,index;
	TimeType min_timestamp, tmp_time;
	int min_times[_MAX_THREADS];
	int min_counter=0,min_served=0;

	volatile NodeArrayBlock * heads_local[_MAX_THREADS];
	volatile NodeArrayBlock * tails_local[_MAX_THREADS];
	int outs_local[_MAX_THREADS];
	int ins_local[_MAX_THREADS];
	TimeType local_timearray[_MAX_THREADS];

	local_timearray[startIndex] = timestamps_arr[startIndex];
	min_timestamp = local_timearray[startIndex];
	heads_local[startIndex] = (NodeArrayBlock *)heads[startIndex];
	outs_local[startIndex]  = outs[startIndex];
	tails_local[startIndex] = (NodeArrayBlock *)tails[startIndex];
	ins_local[startIndex]   = ins[startIndex];

	for(i=startIndex+1,index = startIndex; i < endIndex ; i++){
		heads_local[i] = (NodeArrayBlock *)heads[i];
		outs_local[i]  = outs[i];
		tails_local[i] = (NodeArrayBlock *)tails[i];
		ins_local[i]   = ins[i];
		local_timearray[i] = timestamps_arr[i];
		if (local_timearray[i] < min_timestamp){
			min_timestamp = local_timearray[i];
			index = i;
			min_counter=0;
		}else{
			if (local_timearray[i] == min_timestamp)
				min_times[min_counter++]=i;
		}
	}
	do{
		//if central queue is full 
		while((out_local_index-1)==in_local_index || 
			(out_local_index==0 && in_local_index == HELPERS_QUEUE_SIZE_1) || _gIsStopThreads){
				if (_gIsStopThreads)
					return;
		}

		helper_queue_time[in_local_index] = heads_local[index]->_timestamp_arr[outs_local[index]];
		helper_queue_value[in_local_index] = heads_local[index]->_values_arr[outs_local[index]++];

		if (in_local_index < HELPERS_QUEUE_SIZE_1)
			in_local_index++;
		else
			in_local_index = 0;	


		if (ins_local[index] != outs_local[index] ||
			heads_local[index]!=tails_local[index])
		{
			if (outs_local[index] != _INIT_SIZE){}
			else{			
				heads_local[index]->_free = true;
				heads_local[index] = heads_local[index]->_next;
				outs_local[index] = 0;
			}
			local_timearray[index] = heads_local[index]->_timestamp_arr[outs_local[index]];
		}
		else
		{
			//if it was not the last local queue in the array of snapshots
			if (--endIndex != index){
				heads_local[index]					= heads_local[endIndex];
				tails_local[index]					= tails_local[endIndex];
				outs_local[index]					= outs_local[endIndex];
				ins_local[index]					= ins_local[endIndex];
				local_timearray[index]				= local_timearray[endIndex];
			}
			if ((startIndex+1)<endIndex){}
			else{
				if (startIndex==endIndex){
					return;
				}
				goto SINGLE_QUEUE_CASE;
			}
			min_counter=min_served=0;
		}
		//If a single Queue left, no need to check timestamps
		if (min_counter > min_served)
			index = min_times[min_served++];
		else{
			min_served=min_counter=0;
			min_timestamp = local_timearray[startIndex];
			for(i = startIndex+1,index=startIndex; i < endIndex ;i++){
				tmp_time = local_timearray[i];
				if (tmp_time <= min_timestamp){
					if (tmp_time < min_timestamp){
						index = i;
						min_counter=0;
						min_timestamp = tmp_time;
					}
					else{
						min_times[min_counter++]=i;
					}
				}
			}
		}
	}while(true);
	return;

SINGLE_QUEUE_CASE:
	int out = outs_local[startIndex];
	int in	= ins_local[startIndex];
	volatile NodeArrayBlock* he = heads_local[startIndex];
	volatile NodeArrayBlock* ta = tails_local[startIndex];
	int* value_arr = he->_values_arr;
	TimeType* time_arr = he->_timestamp_arr;
	do{
		if (out != _INIT_SIZE || he==ta){}
		else{
			he->_free = true;
			he = he->_next;
			value_arr = he->_values_arr;
			time_arr = he->_timestamp_arr;
			out = 0;
		}

		if (in == out && he==ta){
			return;
		}
		while((out_local_index-1)==in_local_index || 
			(out_local_index==0 && in_local_index == HELPERS_QUEUE_SIZE_1) || 
			_gIsStopThreads){
				if (_gIsStopThreads)
					return;
		}

		helper_queue_time[in_local_index] = time_arr[out];
		helper_queue_value[in_local_index] = value_arr[out++];
		if (in_local_index < HELPERS_QUEUE_SIZE_1)
			in_local_index++;
		else
			in_local_index = 0;
	}while(true);
}



////////////////////////////////////////////////////////////////////////////////
// File    : AFCQueue.h
// Author  : Mr Michael Gorelik;  email: smgorelik@gmail.com;
// Written : 02 August 2011
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the New BSD License.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of 
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
// General Public License for more details.
//
// You should have raeceived a copy of the GNU General Public License 
// along with this program; if not, write to the Free Software Foundation
// Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
////////////////////////////////////////////////////////////////////////////////

#endif

